
#include <iostream>

#include <Eigen/Dense>


# define M_PI		3.14159265358979323846	/* pi */
constexpr double kDegreeToRadian = M_PI / 180.;
constexpr double kRadianToDegree = 180. / M_PI;

void test_case1() {

//setup
    //Eigen::Vector3d mean_acc(0.0, 0.0, 9.8);
    Eigen::Vector3d mean_acc(9.8/2.1, 89.8/3.9, 9.8/1.2);
    Eigen::Matrix3d *G_R_I = new Eigen::Matrix3d;

//eskf_reference initializer.cpp
{
        // imu坐标系为FLU，世界坐标系为ENU,静止时f = a - g = 0 - g = (0,0,1)
        Eigen::Vector3d acc_norm = mean_acc.normalized();
        double roll = std::atan2(acc_norm.y(), acc_norm.z());
        double pitch = std::asin(-acc_norm.x());
        printf("roll: %f, pitch: %f\n", roll * kRadianToDegree, pitch * kRadianToDegree);

        *G_R_I = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
                 Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
        Eigen::Vector3d acc = (*G_R_I).transpose() * Eigen::Vector3d(0., 0., 1);
        // check acc1 == acc2
        printf("acc1: %f, %f, %f\n", acc.x(), acc.y(), acc.z());
        printf("acc2: %f, %f, %f\n", acc_norm.x(), acc_norm.y(), acc_norm.z());

        std::cout << "G_R_I: \n" << *G_R_I << std::endl;
}
//  initializer.cpp
{
        // Compute rotation.
        // Please refer to 
        // https://github.com/rpng/open_vins/blob/master/ov_core/src/init/InertialInitializer.cpp
        
        // Three axises of the ENU frame in the IMU frame.
        // z-axis.
        const Eigen::Vector3d& z_axis = mean_acc.normalized(); 

        // x-axis.
        Eigen::Vector3d x_axis = 
            Eigen::Vector3d::UnitX() - z_axis * z_axis.transpose() * Eigen::Vector3d::UnitX();
        x_axis.normalize();

        // y-axis.
        Eigen::Vector3d y_axis = z_axis.cross(x_axis);
        y_axis.normalize();

        Eigen::Matrix3d I_R_G;
        I_R_G.block<3, 1>(0, 0) = x_axis;
        I_R_G.block<3, 1>(0, 1) = y_axis;
        I_R_G.block<3, 1>(0, 2) = z_axis;

        *G_R_I = I_R_G.transpose();
        std::cout << "G_R_I: \n" << *G_R_I << std::endl;
}
//cleanup
    delete G_R_I;
}
int main() {

    test_case1();

    return 0;
}